package edu.gmu.projects.util.geo;


import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.List;

//import com.bbn.openmap.geo.Geo;


import edu.gmu.projects.fireresponder.environment.Location;

//  Should use native libraries...
public class CoordinateCalculator {

	private static double earthsRadius = 6371; // km
	private static double pointsInCircle = 8; 

	// 25% faster and slightly more accurate than openmaps
	public static double getDistance(Location pt1, Location pt2) {
		double lat1 = pt1.getLatitude() * Math.PI / 180;
		double lon1 = pt1.getLongitude() * Math.PI / 180;
		double lat2 = pt2.getLatitude() * Math.PI / 180;
		double lon2 = pt2.getLongitude() * Math.PI / 180;

		return Math.acos(Math.sin(lat1)*Math.sin(lat2) +
				Math.cos(lat1)*Math.cos(lat2) *
				Math.cos(lon2-lon1)) * earthsRadius;
	}
	
	//public static double getOpenMapDistance(Location pt1, Location pt2) {
	//	return Geo.distanceKM(pt1.getLatitude(), pt1.getLongitude(), pt2.getLatitude(), pt2.getLongitude());
	//}

	// distance in km
	// bearing in radians
	public static Location getPoint(Location point, double distance, double bearing) {

		// max distance...
		if(distance > 5000) {
			distance = 5000;  
		}
		
		double lat1 = point.getLatitude() * Math.PI / 180;
		double lon1 = point.getLongitude() * Math.PI / 180;

		double dpr = distance / earthsRadius;

		double lat2 = lat1 + dpr * Math.cos(bearing);
		double dLat = lat2 - lat1;
		double dPhi = Math.log(Math.tan(lat2 / 2 + Math.PI / 4)/Math.tan(lat1 / 2 + Math.PI / 4));
		double q = (dPhi != 0.0) ? dLat / dPhi : Math.cos(lat1);  // E-W line gives dPhi=0
		double dLon = dpr * Math.sin(bearing) / q;
		if (Math.abs(lat2) > Math.PI/2) lat2 = lat2 > 0 ? Math.PI-lat2 : -(Math.PI-lat2);
		double lon2 = (lon1 + dLon + 3 * Math.PI) % (2 * Math.PI) - Math.PI;

		Location resultPoint = new Location();
		resultPoint.setLatitude(lat2 * 180 / Math.PI);
		resultPoint.setLongitude(lon2 * 180 / Math.PI);
		
		return resultPoint;
	}

	// distance in km
	// bearing in radians
	public static List<Location> circleToPolygon(Location point, double distance) {

		List<Location> points = new ArrayList<Location>();
		
		for(int i = 0; i < pointsInCircle; i++) {
			points.add(getPoint(point, distance, i * 2 * Math.PI / pointsInCircle));
		}
		
		points.add(points.get(0));
		return points;
	}

	// distance in km
	public static String circleToPolygonGeometryWKT(Location point, double distance) {

		StringBuilder bdr = new StringBuilder();
		DecimalFormat formatter = new DecimalFormat("###0.0#####");
		
		bdr.append("POLYGON( ( ");
		
		for(int i = 0; i < pointsInCircle; i++) {
			Location p = getPoint(point, distance, i * 2 * Math.PI / pointsInCircle);
			bdr.append(formatter.format(p.getLongitude())).append(" ").append(formatter.format(p.getLatitude())).append(" , ");
		}

		Location p = getPoint(point, distance, 0);
		bdr.append(formatter.format(p.getLongitude())).append(" ").append(formatter.format(p.getLatitude()));

		bdr.append(" ) )");
		
		return bdr.toString();
	}	

	public static void main(String[] args) {
		
		Location p = new Location();
		p.setLatitude(36.892055);
		p.setLongitude(-76.43576);
		//p.setLatitude(0);
		//p.setLongitude(0);
		Location p2 = new Location();
		p2.setLatitude(36.192055);
		p2.setLongitude(-76.83576);
		
		String wkt = circleToPolygonGeometryWKT(p, 1);

		for(int distance = 1; distance < 5000; distance *= 2) {
			System.out.println(distance + " = desired distance");
			List<Location> points = circleToPolygon(p, distance);
			for(Location pt : points) {
				//double calcDistance = getOpenMapDistance(p, pt);
				double calc2Distance = getDistance(p, pt);
				//System.out.println("OpenMap: " + calcDistance + " between " + p + " and "  + pt);
				System.out.println("Local:   " + calc2Distance + " between " + p + " and "  + pt);
			}
		}
		System.out.println(wkt);
		
		int testLength = 100000;
		//System.out.println("OpenMap claims this is the distance: " + getOpenMapDistance(p, p2));
		long start = System.currentTimeMillis();
		//for(int i = 0; i < testLength; i++) {
		//	getOpenMapDistance(p, p2);
		//}
		long runtime = System.currentTimeMillis() - start;
		//System.out.println("Took openMaps " + ((double) runtime)/1000d + " seconds to calculate " + testLength + " times");

		System.out.println("Local claims this is the distance: " + getDistance(p, p2));
		start = System.currentTimeMillis();
		for(int i = 0; i < testLength; i++) {
			getDistance(p, p2);
		}
		runtime = System.currentTimeMillis() - start;
		System.out.println("Took local " + ((double) runtime)/1000d + " seconds to calculate " + testLength + " times");
	
	}

}
